///////////////////////////////////////////////////////////
// Project 2 //
// Andrew Hains //
// Joseph Blackwell //
// Robert Smyly //
//////////////////////////
// Roomba Controller //
// ROBOT //
// Provides functions necessary for controlling the robot //
// and the serial port/bluetooth connection. //
///////////////////////////////////////////////////////////

#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <time.h>
#include <pthread.h>
#include "robot.h"
#include "buffer.h"

#define BAUD_RATE B57600
#define WRITE_BUFFER_SIZE 100

static int serial; //file descriptor for the serial port
static pthread_mutex_t serial_mutex; //mutex to protect the serial port
static buffer write_buff; //buffer used to store commands to be sent to the robot
static pthread_t write_thread; //thread used to write to the serial port

//writes the contents of write_buff to the serial port
void *robot_write_thread(void *param);

//initialize serial port and robot
int robot_init(char *port) {
	struct termios config;
	unsigned char full_mode[] = {128, 132}; //command to place robot in full mode

	pthread_mutex_init(&serial_mutex, NULL);
	
	init_buffer(&write_buff, WRITE_BUFFER_SIZE);

	pthread_mutex_lock(&serial_mutex);
	serial = open(port, O_RDWR | O_NOCTTY );
	if(serial < 0) { //return -1 if serial port could not be opened
		return -1;
	}
	config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON);
	config.c_oflag = 0;
	config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
	config.c_cflag &= ~(CSIZE | PARENB);
	config.c_cflag |= CS8;
	config.c_cc[VMIN]  = 1;
	config.c_cc[VTIME] = 0;
	if (cfsetispeed(&config, BAUD_RATE) < 0 || cfsetospeed(&config, BAUD_RATE) < 0) { //return -2 if could not set baud rate
		return -2;
	}
	if(tcsetattr(serial, TCSAFLUSH, &config) < 0) { //return -3 if could not set configurations
		return -3;
	}

	write(serial, full_mode, 2); //send full mode command
	pthread_mutex_unlock(&serial_mutex);

	pthread_create(&write_thread, NULL, robot_write_thread, NULL); //start serial port thread

}

//write the contents of write_buff to the serial port
void *robot_write_thread(void *param) {
	buffer_item item;
	unsigned char *array;
	int size;

	while(1) {
		buffer_remove(&write_buff, &item);
		array = (unsigned char*) item;
		size = array[0]; //first item in array is the length of array (not including [0])
		array += 1;
		pthread_mutex_lock(&serial_mutex);
		write(serial, array, size);
		pthread_mutex_unlock(&serial_mutex);
	}
}
	
//place the forward command into write_buff
void robot_forward() {
	unsigned char forward[] = {5, 145, 0, 255, 0, 255};
	buffer_insert(&write_buff, (buffer_item) forward);
}

//place the backward command into write_buff
void robot_backward() {
	unsigned char backward[] = {5, 145, 255, 156, 255, 156};
	buffer_insert(&write_buff, (buffer_item) backward);
}

//place the rotate left command into write_buff
void robot_left() {
	unsigned char left[] = {5, 145, 0, 100, 255, 156};
	buffer_insert(&write_buff, (buffer_item) left);
}

//place the rotate right command into write_buff
void robot_right() {
	unsigned char right[] = {5, 145, 255, 156, 0, 100};
	buffer_insert(&write_buff, (buffer_item) right);
}

//place the stop command into write_buff
void robot_stop() {
	unsigned char stop[] = {5, 145, 0, 0, 0, 0};
	buffer_insert(&write_buff, (buffer_item) stop);
}

//request value of the bumper sensors and place the results and a timestamp in their respective variables
void robot_bumpers(unsigned char *data, time_t *timestamp) {
	unsigned char bump[] = {2, 142, 7};

	buffer_insert(&write_buff, (buffer_item) bump);
	read(serial, data, 1);
	*timestamp = time(NULL); //record the time this function was run
}

//deletes the buffer and closes the serial port in preparation for ending the program
int robot_close() {
	destroy_buffer(&write_buff);
	return close(serial); //close the serial port
}
